#include "em80_interface.h"

#include <pthread.h>

#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>  //fopen,...

#include <serial/serial.h>
#include <boost/crc.hpp>


namespace em80_driver
{

CommandAdapter::CommandAdapter() : command_buffer(new EM80Packet(base_length + extend_length)),
                                   base_control(command_buffer),
                                   docking_config(command_buffer),
                                   led_config(command_buffer),
                                   gpio_config(command_buffer),
                                   adc_config(command_buffer),
                                   dac_config(command_buffer),
                                   power_source_config(command_buffer),
                                   joystick_mode(command_buffer),
                                   fw_version_enquiry(command_buffer),
                                   docking_enquiry(command_buffer),
                                   status_enquiry(command_buffer)

{

    *(command_buffer->packet().first - 1) = base_length + extend_length;         //TODO: TOO UGLY!! and the length should be varied with the extra request!!
    command_buffer->pushPayload(EM80PayloadBasePtr(&base_control));
    command_buffer->pushPayload(EM80PayloadBasePtr(&docking_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&led_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&gpio_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&adc_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&dac_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&power_source_config));
    command_buffer->pushPayload(EM80PayloadBasePtr(&joystick_mode));
    command_buffer->pushPayload(EM80PayloadBasePtr(&fw_version_enquiry));
    command_buffer->pushPayload(EM80PayloadBasePtr(&docking_enquiry));
    command_buffer->pushPayload(EM80PayloadBasePtr(&status_enquiry));

}

void CommandAdapter::SetVelocityControl(const int16_t &left_wheel_speed, const int16_t &right_wheel_speed)
{
    base_control.data.leftwheel_speed = left_wheel_speed;
    base_control.data.rightwheel_speed = right_wheel_speed;
}

void CommandAdapter::SetDocking(const DockingConfig::Mode::State &mode)
{
    docking_config.data.mode = static_cast<const uint8_t>(mode);
}

void CommandAdapter::SetLEDArray(const LEDConfig::Mode::State &mode)
{
    led_config.data.mode = static_cast<const uint8_t>(mode);
}

void CommandAdapter::SetGPIO(const DigitalIO &gpio) 
{
    uint8_t value = 0x00;
    uint8_t output_mask = 0xff;
    for(unsigned int i = 0; i < 8; ++i)
    {
        if(gpio.mask[i])
        {
            if(gpio.values[i])
                value |= (1 << i);
            output_mask |= (1 << i);
        }
    }

    gpio_config.data.io_config = output_mask;
    gpio_config.data.value = value;
}

void CommandAdapter::SetGPIO(const uint8_t &output_mask, const uint8_t &value)
{
    gpio_config.data.io_config = output_mask;
    gpio_config.data.value = value;
}

void CommandAdapter::SetADC(const ADCConfig::Control::State &control)
{
    adc_config.data.adc = static_cast<const uint8_t>(control);
}

void CommandAdapter::SetDAC(const DACConfig::Control::State &control, uint16_t values)
{
    dac_config.data.dac = static_cast<const uint8_t>(control);
    dac_config.data.value = values;
}

void CommandAdapter::SetExternalPower(const DigitalIO &power_source)
{
    uint8_t values = 0x00;
    for(unsigned int i = 0; i < 8; ++i)
    {
        if (power_source.values[i])
            values |= (1 << i);
    }

    power_source_config.data.flags = values;
}

void CommandAdapter::SetExternalPower(const uint8_t &power_source)
{
    power_source_config.data.flags = power_source;
}

void CommandAdapter::SetJoystick(const JoystickMode::Mode::State &mode)
{
    joystick_mode.data.mode = mode;
}

void CommandAdapter::GetExtended()
{}


class EM80Interface::Impl
{
public:
    Impl() : serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100),
                     serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none)
    {}

    void* rxThread();


    //static entrance for different rx thread usr logic
    static void* rxThreadHelper(void* context)
    {
        return ((EM80Interface::Impl*)context)->rxThread();
    }

    pthread_t rx_thread_;
    bool rx_thread_run_;
    PayloadHandlerFunction payload_handler_;
    ErrorHandlerFunction error_handler_;
    serial::Serial serial_;
    EM80Frame::CRC send_crc_;
};

void* EM80Interface::Impl::rxThread()
{
    Buffer buffer;
    buffer.reserve(4096);

    while(rx_thread_run_)
    {
        int bytes_needed = EM80Frame::EM80_MIN_FRAME_SIZE;

        if(!buffer.empty())
        {
            // search buffer for valid packet(s)
            Buffer::iterator iter(buffer.begin());
            Buffer::iterator iter_begin(buffer.begin());
            while(iter != buffer.end())
            {
                // check if valid start-of-fram character
                if (EM80Frame::EM80_SOF_FRIST_VAL_FRAME == *iter ||
                    EM80Frame::EM80_SOF_SECOND_VAL_FRAME == *(iter+1))
                {

                    std::string error;
                    EM80PacketPtr packet = generateEM80Packet(iter, buffer.end(), &bytes_needed, &error);

                    if(packet)
                    {
                        //good packet, check if we skipped any data
                        if(std::distance(iter_begin, iter) > 0)
                        {
                            std::ostringstream ss;
                            ss << "Out-of-sync with EM80, unknown data leading valid frame. Discarding "
                              << std::distance(iter_begin, iter) << " bytes.";
                            error_handler_(ss.str());
                        }

                        // **generating the payload(s) for the "factory" & put it in the payloadList_
                        // TODO: no error/payload handlers are passed to the EM80Packet.

                        bool packet_deserialized = packet->deserialize();
/*
                        for(auto iter = packet->packet().first; iter != packet->packet().second; iter++)
                        {
                            printf("%x \t", *iter);
                        }

                        printf("\n");
*/
                        // call packet handler --> call payload handler to the payloadList_
                        if (packet_deserialized)
                        {
                            //printf("payload handler with payloadList_ size (%d) \n", packet->payloadList_.size());
                            for (auto iter = packet->payloadList_.begin(); iter != packet->payloadList_.end(); ++iter)
                            {
                                EM80PayloadBaseConstPtr tmp = *iter;
                                payload_handler_(tmp);
                            }
                            //printf("payload handler with payloadList_ size (%d) \n", packet->payloadList_.size());
                        }

                        packet->payloadList_.clear();
                        //printf("payload handler with payloadList_ size (%d) \n", packet->payloadList_.size());

                        // update state (checking the next packet
                        iter = iter + packet->frame().size();
                        iter_begin = iter;

                        // continue to look for another frame in buffer
                        continue;
                    }
                    else if (bytes_needed > 0)
                    {
                        // need more data(the buffer is not containing enough raw data, break out of while loop that scan the buffer
                        //printf("we need more bytes from serial. \n");
                        break;
                    }
                    else
                    {
                        // else, this was not a valid packet, we need to move on to next byte
                        error_handler_(error);
                    }

                }  // if (EM80Frame::EM80_SOF_FRIST_VAL_FRAME == *begin ||
                   //     EM80Frame::EM80_SOF_SECOND_VAL_FRAME == *(begin+1))

                iter++;
            } //while(iter != buffer.end())

            // if iter is at the end of the buffer, more bytes are needed
            if (iter == buffer.end())
                bytes_needed = EM80Frame::EM80_MIN_FRAME_SIZE;

            // erase "used" buffer
            if (std::distance(iter_begin, iter) > 0)
            {
                std::ostringstream ss;
                ss << "Out-of-sync DACConfigConstPtrith EM80, discarding " << std::distance(iter_begin, iter) << " bytes.";
                error_handler_(ss.str());
            }

            buffer.erase(buffer.begin(), iter);
        } // if(!buffer.empty())


        // attempt to read at leastDACConfigConstPtrbytes_needed bytes from the serial port
        int bytes_to_read = std::max(bytes_needed, std::min(4096, static_cast<int>(serial_.available())));


        int bytes_read = serial_.read(buffer, bytes_to_read);

///////////////////////////////////////////////////////////////////////////////
        //printf("we need %d bytes \n", bytes_to_read);
        //printf("serial read %d bytes \n", bytes_read);
	
        /*
        printf("\n");

        printf("******************************************************");
	printf("\n");
        for(auto iter= buffer.begin(); iter != buffer.end(); iter++)
        {
            printf("%x \t", *iter);
        }
	printf("\n");
	printf("******************************************************");
       	printf("\n");
	*/




        if(bytes_needed > 0  && 0 == bytes_read && !buffer.empty())
        {
            error_handler_("Possibly out-of-sync with EM80, read timeout in the middle of a frame");
        }

    } // while (rx_thread_run_)

    return static_cast<void*>(0);
}

EM80Interface::EM80Interface(const std::string &port,
                             const PayloadHandlerFunction &payload_handler,
                             const ErrorHandlerFunction &error_handler) :
    impl_(new Impl())
{
    setPayloadHandler(payload_handler);
    setErrorHandler(error_handler);
    // attempt to connect if the port is specified
    if(!port.empty())
        connect(port);
}

EM80Interface::~EM80Interface()
{
    disconnect();
}

void EM80Interface::setPayloadHandler(const PayloadHandlerFunction &handler)
{

    // todo - defintely need mutex
    impl_->payload_handler_ = handler;
}


void EM80Interface::setErrorHandler(const ErrorHandlerFunction &handler)
{
    // todo - defintely need mutex
    impl_->error_handler_ = handler;
}

void EM80Interface::connect(const std::string &port)
{
    // TODO - mutex ?
    if(isConnected())
    {
        throw SerialException("Already connected to serial port.");
    }


    try
    {
        impl_->serial_.setPort(port);  //already new one Serial instance while impl_ is constructed
        impl_->serial_.open();
    }
    catch (const std::exception &e)
    {
        std::stringstream ss;
        ss << "Failed to open the serial port to the EM80. " << e.what();
        throw SerialException(ss.str().c_str());
    }

    // start up a monitoring thread
    impl_->rx_thread_run_ = true;

    int result = pthread_create(&impl_->rx_thread_, NULL, &EM80Interface::Impl::rxThreadHelper, impl_.get());

    assert(0 == result);
}

void EM80Interface::disconnect()
{
    // TODO - mutex ?

    if(isConnected())
    {
        //bring down read thread
        impl_->rx_thread_run_ = false;
        int result = pthread_join(impl_->rx_thread_, NULL);
        assert(0 == result);

        impl_->serial_.close();
    }
}


bool EM80Interface::isConnected() const
{
    return impl_->serial_.isOpen();
}

void EM80Interface::sendCommand()
{
    bool success = command.command_buffer->serialize();
    

    if (success)
    {
        size_t written = impl_->serial_.write(command.command_buffer->frame());

        /*
        for(auto iter = command.command_buffer->frame().begin(); iter != command.command_buffer->frame().end(); iter++)
        {
            printf("%x \t", *iter);
        }
        printf("\n");
        */
        if(written != command.command_buffer->frame().size())
        {
            std::stringstream ss;
            ss << "wrote " << written << " bytes, expected " << command.command_buffer->frame().size() << ".";
            throw SerialException(ss.str().c_str());
        }
    }
}


// EM80 command API
void EM80Interface::SetVelocityControl(const int16_t &left_wheel_speed, const int16_t &right_wheel_speed)
{
    command.SetVelocityControl(left_wheel_speed, right_wheel_speed);
}

void EM80Interface::SetDocking(const DockingConfig::Mode::State &mode)
{
    command.SetDocking(mode);
}

void EM80Interface::SetLEDArray(const LEDConfig::Mode::State &mode)
{
    command.SetLEDArray(mode);
}

void EM80Interface::SetGPIO(const DigitalIO &gpio)
{
    command.SetGPIO(gpio);
}

void EM80Interface::SetGPIO(const uint8_t &output_mask, const uint8_t &value)
{
    command.SetGPIO(output_mask, value);
}

void EM80Interface::SetADC(const ADCConfig::Control::State &control)
{
    command.SetADC(control);
}

void EM80Interface::SetDAC(const DACConfig::Control::State &control, uint16_t values)
{
    command.SetDAC(control, values);
}

void EM80Interface::SetExternalPower(const DigitalIO &power_source)
{
    command.SetExternalPower(power_source);
}

void EM80Interface::SetExternalPower(const uint8_t &power_source)
{
    command.SetExternalPower(power_source);
}

void EM80Interface::SetJoystick(const JoystickMode::Mode::State &mode)
{
    command.SetJoystick(mode);
}

void EM80Interface::GetExtended()
{
    command.GetExtended();
}

} // namespace em80_driver
